#include <os/fifoio.h>
#include <os/fifo.h>
#include <os/mutexlock.h>
#include <os/semaphore.h>
#include <lib/math.h>
#include <lib/type.h>
#include <lib/string.h>

// alloc fifo buffer
fifo_io_t *FifoIoAlloc(uint32_t size)
{
    fifo_io_t *fifo = NULL;
    uint8_t *buff = NULL;

    if (!IS_POWER_OF_2(size))
    {
        size = roundup_pow_of_two(size);
    }

    buff = (uint8_t *)KMemAlloc(size);
    if (!buff)
    {
        KPrint("[fifo] alloc size %d for fifo failed!\n", size);
        return NULL;
    }
    // alloc fifo io object
    fifo = (fifo_io_t *)KMemAlloc(sizeof(fifo_io_t));
    if (!fifo)
    {
        KMemFree(buff);
        memset(fifo->buffer, 0, size);
        return NULL;
    }
    // fifo io init
    if (!FifoIoInit(fifo, buff, size))
        return fifo;
    KMemFree(buff);
    KMemFree(fifo);
    return NULL;
}

// free fifo buffer
void FifoIoFree(fifo_io_t *fifo)
{
    KMemFree(fifo->buffer);
    memset(fifo->buffer, 0, fifo->size);
    KMemFree(fifo);
}

// init fifo buffer
int FifoIoInit(fifo_io_t *fifo, uint8_t *buff, uint32_t size)
{
    if (!IS_POWER_OF_2(size))
    {
        return -1;
    }
    // flush buffer
    memset(fifo->buffer, 0, size);
    // set argument
    fifo->buffer = buff;
    fifo->size = size;
    fifo->count = 0;
    fifo->head = fifo->tail = fifo->buffer;
    // init semaphore and mutexlock
    SemaphoreInit(&fifo->empty, size);
    SemaphoreInit(&fifo->full, 0);
    MutexlockInit(&fifo->mutex);
    return 0;
}

// put data to fifo buffer
void FifoIoPut(fifo_io_t *fifo, uint8_t data)
{
    SemaphoreDown(&fifo->empty);
    MutexlockLock(&fifo->mutex, MUTEX_LOCK_MODE_BLOCK);

    if (!FifoIoAvailable(fifo))
    {
        *(uint8_t *)fifo->tail++ = data;
        if (fifo->tail >= fifo->buffer + fifo->size)
            fifo->tail = fifo->buffer;
        fifo->count++;
        //KPrint("fifo put %d count %d\n",*((uint8_t*)fifo->tail-1),fifo->count);
    }

    MutexlockUnlock(&fifo->mutex);
    SemaphoreUp(&fifo->full);
}

// get data frome fifo buffer
uint8_t FifoIoGet(fifo_io_t *fifo)
{
    uint8_t data = 0;

    SemaphoreDown(&fifo->full);
    MutexlockLock(&fifo->mutex, MUTEX_LOCK_MODE_BLOCK);

    if (FifoIoLen(fifo) > 0)
    {
        data = *(uint8_t *)fifo->head++;
        if (fifo->head >= fifo->buffer + fifo->size)
            fifo->head = fifo->buffer;
        fifo->count--;
    }

   //KPrint("fifo get %d count %d\n",*((uint8_t*)fifo->head-1),fifo->count);

    MutexlockUnlock(&fifo->mutex);
    SemaphoreUp(&fifo->empty);
    return data;
}
